// Arm class header file.


#ifndef _ARM_
#define _ARM_

#include "WProgram.h"
#include "../Servo/Servo.h"

typedef struct {
	//These are all relative to 0,0,0 located at
        //the center of the base of the arm
	double tcp_x;
	double tcp_y;
	double tcp_z;
}_tcp;

typedef struct {
	Servo* servo;
	
	//The following values are in micro-seconds.
	unsigned int max;
	unsigned int min;
	unsigned int neutral;
	unsigned int calibPos;
}_axis;

typedef _tcp* _tcp_ptr; //Don't know if I'm going to need this yet.
typedef _axis* _axis_ptr;

class Arm
{
	public:
		Arm();
		void initArm();
		//This function will allow initialization of an
		//arm with an arbitrary number of axies.
		//void initArmWithLimits( int numAxis, int[][ 2 ] limits );	
		//Getters
		void getTCP( double tcp[ 3 ] );//Must be of size 3
		double getTCPX();
		double getTCPY();
		double getTCPZ();

		void setTCP( double x, double y, double z, double grip_angle);
		void goToCal();
		int setAxisAngle( int axisNum, int angle );
		void gripperOpen();
		void gripperClose();
		void goHome();
	private:
		//Data

		//Constants
		static const unsigned int _NUM_AXIS = 6;
		unsigned int _ARM_LIMITS[ _NUM_AXIS ][ 4 ]; //Need to figure out init
		//Dimensions are in mm and are unconfirmed.
		static const double _base_height = 76.2;
		static const double _link_2_3 = 121.0;
		static const double _link_3_4 = 121.0;
		static const double _link_4_5 = 110.0;;
	
		//For now I'm going to make the max min values part of the lib
		//Will want to make these configurable in the future.

		int _gripper_open; //Important for IK calculation.
		_tcp_ptr _tool_loc;
		_axis_ptr _axis_array[ _NUM_AXIS ];

		//Utility functions
};


#endif
